/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received _a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#pragma once

#include "Chain.h"

namespace RobotPlant { namespace Mechanism
{
	using namespace RobotPlant::Math;

	/*
	The MotionPlanning is used to control robot's motion according to input.
	In the whole world, there are several frames as below:
	WorldFrame: it's the world coordinate system, top level frame, always be Identity frame.
	RobotBaseFrame: it's the robot's base frame w.r.t WorldFrame;
	ObjectFrame: it's the motion command input target frame's parent frame. It's w.r.t WorldFrame.
	ToolFrame: it's the robot's tool frame w.r.t robot's end effector frame. Motion commands moves robot to make ToolFrame in the pose of ObjectFrame.
	*/
	class RP_MECHANISM_API MotionPlanning
	{
	public:
		MotionPlanning(Chain* pRobot, double stepInterval = 0.01);

		/*
		Destructor
		*/
		virtual ~MotionPlanning();

		/*
		@brief: gets the time interval between two steps, unit is s.
		@returns: returns current robot's joint values.
		*/
		double GetStepInterval();

		/*
		@brief: gets the minimum flybyzone, unit is m.
		@returns: returns current minimum flybyzone.
		*/
		double GetMinFlyByZone();

		/*
		@brief: gets robot base frame w.r.t global frame.
		@returns: returns robot base frame.
		*/
		Frame3 GetRobotBaseFrame();

		/*
		@brief: gets target pose w.r.t object frame.
		@returns: returns object frame.
		*/
		Frame3 GetObjectFrame();

		/*
		@brief: gets robot's tool frame w.r.t robot's end effector.
		@returns: returns tool frame.
		*/
		Frame3 GetToolFrame();

		/*
		@brief: gets current robot joint values.
		@returns: returns current robot joint values.
		*/
		ColumnVector GetCurRobotJointValues();

		/*
		@brief: gets current robot joint velocities.
		@returns: returns current robot joint velocities.
		*/
		ColumnVector GetCurRobotJointVels();

		/*
		@brief: gets current robot joint accelerations.
		@returns: returns current robot joint accelerations.
		*/
		ColumnVector GetCurRobotJointAccs();

		/*
		@brief: gets current robot joint torques out put from gearbox.
		@returns: returns current robot joint torques out put from gearbox.
		*/
		ColumnVector GetCurRobotJointTorques();

		/*
		@brief: sets the time interval between two steps, unit is s.
		@param: interval is the time interval between two steps, unit is s.
		*/
		void SetStepInterval(double interval);

		/*
		@brief: sets the minimum flybyzone, unit is m.
		@param: minFlyByZone is the minimum flyby zone, if it's < 0.001, will be set as 0.001.
		*/
		void SetMinFlyByZone(double minFlyByZone);

		/*
		@brief: sets robot base frame.
		@param: baseFrame is the input robot base frame.
		*/
		void SetRobotBaseFrame(Frame3 baseFrame);

		/*
		@brief: sets object frame.
		@param: objectFrame is the input object frame.
		*/
		void SetObjectFrame(Frame3 objectFrame);

		/*
		@brief: sets robot's tool frame.
		@param: toolFrame is the input tool frame.
		*/
		void SetToolFrame(Frame3 toolFrame);

		/*
		@brief: sets current robot's joint values.
		@param: values is the input current robot's joint values.
		*/
		void SetCurRobotJointValues(ColumnVector values);

		/*
		@brief: sets current robot's joint velocities.
		@param: values is the input current robot's joint velocities.
		*/
		void SetCurRobotJointVels(ColumnVector values);

		/*
		@brief: sets current robot's joint accelerations.
		@param: values is the input current robot's joint accelerations.
		*/
		void SetCurRobotJointAccs(ColumnVector values);

		/*
		@brief: sets current robot's joint torques.
		@param: values is the input current robot's joint torques.
		*/
		void SetCurRobotJointTorques(ColumnVector values);

		/*
		@brief: moves robot tool frame with specified velocity.
		@param vel: desired tool frame move velocity.
		@param global: if true, vel is w.r.t global frame, else it's w.r.t tool frame.
		*/
		virtual void MoveForward(Velocity vel, bool global = false) = 0;

		/*
		@brief: moves robot to make all joint values as toJointValues.
		@param toPoint: desired target joint values in joint space.
		@param speed: desired motion speed of joints. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in Cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot all joints will reach and stop at toPoint.
		*/
		virtual void MoveJoint(const ColumnVector toJointValues, double speed, double flyByZone = 0) = 0;

		/*
		@brief: moves robot tool frame to toPoint w.r.t object frame.
		@param toPoint: desired target frame w.r.t object frame.
		@param speed: desired motion speed of joints. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot tool frame will reach and stop at toPoint.
		*/
		virtual void MoveCart(const Frame3 toPoint, double speed, double flyByZone = 0) = 0;

		/*
		@brief: moves robot tool frame to toPoint w.r.t object frame and make the path linearly.
		@param toPoint: desired target frame w.r.t object frame.
		@param speed: desired motion speed of ToolFrame. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot tool frame will reach and stop at toPoint.
		*/
		virtual void MoveCart_Linear(const Frame3 toPoint, double speed, double flyByZone = 0) = 0;

		/*
		@brief: stops robot motion.
		@param followPath: true means robot will move along current path and reduce motion speed until 0;
		false means robot will stop as soon as possible without follow current path.
		*/
		virtual void Stop(const bool followPath) = 0;

		/*
		@brief: moves robot to next step and save joint values, velocities, accelerations and torques.
		@return: returns >0 means this motion step successful, 0 means motion completed and stopped, <0 means failed.
		@remark: after invokes this method, gets joint values, velocities, accelerations and torques with method GetCurRobotJointValues(), GetCurRobotJointVels(), GetCurRobotJointAccs(), GetCurRobotJointTorques().
		*/
		virtual int NextStep() = 0;

	protected:
		Chain* _pRobot;//pointer to robot to be controlled.		

		double _stepInterval;//!< the time interval between two steps, unit is s.

		double _minFlyByZone;// the minimum flyby zone.

		Frame3 _robotBaseFrame;//robot base frame fixed in whole environments.

		Frame3 _objectFrame;//object frame fixed in whole environments. All target frames are w.r.t to this frame.

		Frame3 _toolFrame;//tool frame mounted on robot end effector. Robot motion is moving this tool frame to target frame w.r.t object frame.

		ColumnVector _curRobotJointValues;//current robot joint value

		ColumnVector _curRobotJointVels;//current robot joint velocity

		ColumnVector _curRobotJointAccs;//current robot joint acceleration

		ColumnVector _curRobotJointTorques;//current robot joint torque, output from gearbox

	};

}}